//
// Created by TheHun on 2020/10/7.
//
#include <math.h>
#include <stdio.h>
#include "..\\code\\dataStruct.h"
#include "..\\code\\common.h"

int lineSingleRecloseStarter(Device* device);
int lineTripleRecloseStarter(Device* device);
extern int TWJ(Device* device, int phase);
int synReclosePredicate(double angleDiff, double slipDiff, double closeTime, double angleDiffSet);
extern void reSetAllTripFlag(Device* device, int phase);
void Enqueue(int* queue, int size, int* head, int val);
int Dequeue(int* queue, int* head);


/**
 * 线路重合闸执行函数
 * 电压等级：220kV
 * @param device
 */
void lineRecloser_220kV(Device* device) {
    // 停用重合闸、禁止重合闸-->直接返回
    if (device->stopRecloseMode == 1 || device->banRecloseMode == 1) {
        return;
    }
    const int RECLOSE_KEEP_TIME = 0.02;

    if ((fabs(device->recloseOpTime[0]) > 0.001) || (fabs(device->recloseOpTime[1]) > 0.001) || (fabs(device->recloseOpTime[2]) > 0.001)) {
        if (device->time - device->recloseOpTime[0] > RECLOSE_KEEP_TIME) {
            device->recloseOpTime[0] = device->recloseOpTime[1] = device->recloseOpTime[2] = -0.002;
            device->recloseFlag[0] = device->recloseFlag[1] = device->recloseFlag[2] = 0; // 只重合一次
            return;
        } else {
            reSetAllTripFlag(device, 0); reSetAllTripFlag(device, 1); reSetAllTripFlag(device, 2);
            device->tripFlag[0] = device->tripFlag[1] = device->tripFlag[2] = 0;
            return;
        }
    }

    int singleStart = lineSingleRecloseStarter(device); // M4
    int tripleStart = lineTripleRecloseStarter(device); // M10

    int blockReclose; // 闭锁重合闸
    int phase = 0;
    // 对于单重方式，只考虑一相重合闸动作，任一相标志位为1视为重合闸已经动作，不考虑其他情况
    // 对于三重方式，只考虑三相重合闸同时动作，任一相标志位为1视为重合闸已经动作
    int reclose = device->recloseFlag[0] || device->recloseFlag[1] || device->recloseFlag[2];
    blockReclose = !(singleStart || tripleStart) || device->recloseBlocked || reclose;
    int lineNoVoltage = !(device->synPTBreakTripFlag) && (phasorAbs(device->busPhasor[0]) < 0.03); // M13,需要改成检查同期电压断线的标志位(已改)
    int checkNoVoltage = lineNoVoltage || (phasorAbs(device->busPhasor[0]) < 0.03); // M14

    // 角差整定值
    double angleDiffSet = device->synRecloseAngleSetValue;
    int synFlag = (fabs(angleDiffCalculate(device->phasor[0], device->busPhasor[0])) < angleDiffSet);
    int checkSyn = (phasorAbs(device->phasor[0]) > 40) && (phasorAbs(device->busPhasor[0]) > 40) && synFlag; // M19

    int M20 = !(device->brkStatus[0][0]) || !(device->brkStatus[0][1]) || !(device->brkStatus[0][2]); // M20
    int M21 = 1;//!M20 && !(device->startFlag);

    // 重合闸时间整定值Tcd
    double recloseTimeSetValue = 0.02;
    int blockOut = 0;

    blockOut = Dequeue(device->blockRecloseFlag, &(device->blockHead));
    Enqueue(device->blockRecloseFlag, 1*POINTS, &(device->blockHead), (int)(M21 && !blockReclose));

    int singleRecloseOut = singleStart && blockOut; // M5
    int tripleReclosePre = tripleStart && blockOut; // M11

    int ifCheckNoVoltage = device->checkVoltMode && tripleReclosePre && checkNoVoltage; // M16
    int ifCheckSyn = device->checkSynMode && tripleReclosePre && checkSyn; // M17

    int tripleRecloseOut = ifCheckNoVoltage || ifCheckSyn || (tripleReclosePre && !(device->checkVoltMode && tripleReclosePre) && !(device->checkSynMode && tripleReclosePre)); // M18

    if (device->singleRecloseTripFlag == 1) {
        singleRecloseOut = 1;
    }
    if (device->tripleRecloseTripFlag == 1) {
        tripleRecloseOut = 1;
    }
    if (device->singleRecloseTripFlag == 0 && singleRecloseOut == 1) {
        device->singleRecloseTripFlag = 1;
    } 
    if (device->tripleRecloseTripFlag == 0 && tripleRecloseOut == 1) {
        device->tripleRecloseTripFlag = 1;
    }

    int singleRecloseSignal = 0;
    int tripleRecloseSignal = 0;
    delayJudge(singleRecloseOut, &device->recloseStartTime[1], device->time, device->singleRecloseTimeSet, &singleRecloseSignal);
    delayJudge(tripleRecloseOut, &device->recloseStartTime[2], device->time, device->threeRecloseTimeSet, &tripleRecloseSignal);

    if (singleRecloseSignal || tripleRecloseSignal) {
        device->recloseFlag[0] = device->recloseFlag[1] = device->recloseFlag[2] = 1;
        device->tripFlag[0] = device->tripFlag[1] = device->tripFlag[2] = 0;
        device->recloseOpTime[0] = device->recloseOpTime[1] = device->recloseOpTime[2] = device->time;
        device->reCloseTimes = 1;
        writeLog(device,"重合闸动作");
    }

}

/**
 * 重合闸实现逻辑-110kV
 * @param device
 */
void lineRecloser_110kV(Device* device) {
    // 停用重合闸、禁止重合闸、闭锁重合闸-->直接返回
    if (device->stopRecloseMode == 1 || device->banRecloseMode == 1 || device->recloseBlocked == 1) {
        return;
    }

    // 重合闸之后不再重合
    if ((device->recloseOpTime[0] > 0.001) || (device->recloseOpTime[1] > 0.001) || (device->recloseOpTime[2] > 0.001)) {
        if (device->time - device->recloseOpTime[0] > 0.15) {
            device->recloseOpTime[0] = device->recloseOpTime[1] = device->recloseOpTime[2] = 0.002;
            device->recloseFlag[0] = device->recloseFlag[1] = device->recloseFlag[2] = 0;
            return;
             // 只重合一次
        } else {
            reSetAllTripFlag(device, 0); reSetAllTripFlag(device, 1); reSetAllTripFlag(device, 2);
            device->tripFlag[0] = device->tripFlag[1] = device->tripFlag[2] = 0;
            return;
        }
    }

    // TWJ + 装置未启动 + 保护/同期PT断线
    int TWJ = device->brkStatus[0][0] && device->brkStatus[0][1] && device->brkStatus[0][2];
    int position = TWJ;
    position = 1;
    // 信号闭锁部分
    int relayTrip = device->tripFlag[0] || device->tripFlag[1] || device->tripFlag[2]; // 保护跳闸
    int noCurrent = allPhaseNull(device->phasor, 3, device->noCurrentSet); // 三相均无流

    int trip = relayTrip && noCurrent;
    int block = ((device->synPTBreakTripFlag || device->PTBreakPreFlag) && device->PTBreakBlockReclose) || device->CTBreakFlag || !trip;
    int blockOut = 0;

    blockOut = Dequeue(device->blockRecloseFlag, &(device->blockHead));
    Enqueue(device->blockRecloseFlag, 1*POINTS, &(device->blockHead), position && !block);

    int start = blockOut && trip;

    int synCheckMode[5] = {0}; // 0-不检方式，1-检母无压线有压，2-检线无压母有压，3-检线无压母无压，4-检同期

    synCheckMode[4] = device->checkSynMode;
    synCheckMode[device->checkVoltMode] = 1;
    synCheckMode[0] = ! (device->checkSynMode || device->checkVoltMode);

    double synVolt = phasorAbs(device->busPhasor[0]);
    double relayVolt = phasorAbs(device->phasor[0]);

    int notCheck = synCheckMode[0] && start;
    int checkBus = (!device->PTBreakFlag && (relayVolt < 0.03) && (synVolt > 0.04)) && start && synCheckMode[1];
    int checkLine = (!device->synPTBreakTripFlag && (synVolt < 0.03) && (relayVolt > 0.04)) && start && synCheckMode[2];
    int checkBoth = (!device->PTBreakFlag && (relayVolt < 0.03)) && (!device->synPTBreakTripFlag && (synVolt < 0.03)) && start && synCheckMode[3];
    double angleDiffSet = device->synRecloseAngleSetValue;
    int synFlag = (fabs(angleDiffCalculate(device->phasor[0], device->busPhasor[0])) < angleDiffSet);
    int checkSyn = ((synVolt > 0.04) && (relayVolt > 0.04) && synFlag) && start && synCheckMode[4];

    int ans = notCheck || checkBus || checkLine || checkBoth || checkSyn;
    int finalSignal = 0;
    delayJudge(ans, &device->recloseStartTime[0], device->time, device->threeRecloseTimeSet, &finalSignal);


    if (finalSignal) {
        device->recloseFlag[0] = device->recloseFlag[1] = device->recloseFlag[2] = 1;
        device->tripFlag[0] = device->tripFlag[1] = device->tripFlag[2] = 0;
        device->recloseOpTime[0] = device->recloseOpTime[1] = device->recloseOpTime[2] = device->time;
        reSetAllTripFlag(device, 0); reSetAllTripFlag(device, 1); reSetAllTripFlag(device, 2);
        writeLog(device,"重合闸动作");
    }
}

/**
 * 入队函数
 * @param queue 队列数组
 * @param size 队列长度
 * @param head 队列头
 * @param val 待入队值
 */
void Enqueue(int* queue, int size, int* head, int val) {
    queue[(*head)] = val;
    (*head)++;
    (*head) = (*head) % size;
}

/**
 * 出队函数
 * @param queue 队列数组
 * @param head 头指针
 * @return 最旧的节点值
 */
int Dequeue(int* queue, int* head) {
    return queue[(*head)];
}

/**
 * 单相重合闸启动判断函数
 * @param device
 * @return 单相重合闸启动判断结果
 */
int lineSingleRecloseStarter(Device* device) {
    int brkStart = (!(device->brkStatus[0][0]) || !(device->brkStatus[0][1]) || !(device->brkStatus[0][2])) && device->spTWJRecloseEnable; // M1
    int relaySingleTrip = TWJ(device, 0) || TWJ(device, 1) || TWJ(device, 2); // 本保护单跳固定
    int relayTripleTrip = TWJ(device, 0) && TWJ(device, 1) && TWJ(device, 2); // 本保护三跳固定
    int start = (relaySingleTrip || brkStart) && device->spRecloseMode; // M3
    int anyNoCurrent = anyPhaseNull(device->phasor, 3, device->noCurrentSet); // 任一相无流
    int allNoCurrent = allPhaseNull(device->phasor, 3, device->noCurrentSet); // 三相均无流
    int threeBlock = (!(device->brkStatus[0][0]) && !(device->brkStatus[0][1]) && !(device->brkStatus[0][2])) && allNoCurrent; // M7

    int ret = start && anyNoCurrent && (!relayTripleTrip) && (!threeBlock); // M4
    return ret;
}

/**
 * 三相重合闸启动判断函数
 * @param device
 * @return 三相重合闸启动判断结果
 */
int lineTripleRecloseStarter(Device* device) {
    int allNoCurrent = allPhaseNull(device->phasor, 3, device->noCurrentSet); // 三相均无流
    int relayTripleTrip = TWJ(device, 0) && TWJ(device, 1) && TWJ(device, 2); // 本保护三跳固定
    int threeBlock = (!(device->brkStatus[0][0]) && !(device->brkStatus[0][1]) && !(device->brkStatus[0][2])) && allNoCurrent && device->tpTWJRecloseEnable; // M7
    int ret = (threeBlock || relayTripleTrip) && allNoCurrent && device->tpRecloseMode; // M10
    return ret;
}

/**
 * 同期合闸判断函数
 * @param angleDiff 当前角差
 * @param slipDiff 当前滑差
 * @param closeTime 合闸时间
 * @param angleDiffSet 合闸允许角差门槛值
 * @return
 */
int synReclosePredicate(double angleDiff, double slipDiff, double closeTime, double angleDiffSet) {

    double closeAngleDiff = angleDiff + slipDiff * closeTime;
    if (closeAngleDiff > 180.0) {
        closeAngleDiff -= 360.0;
    } else if (closeAngleDiff <= -180.0) {
        closeAngleDiff += 360.0;
    }
    if (fabs(closeAngleDiff) <= angleDiffSet) {
        return 1;
    }
    return 0;
}


/**
 * TWJ: 位置不对应 + 本相无流
 */
int TWJ(Device* device, int phase) {
    if (device->manBrkStatus[0][phase] == 1 && device->brkStatus[0][phase] == 0 && phasorAbs(device->phasor[3 + phase]) < device->noCurrentSet) {
        return 1;
    } else {
        return 0;
    }
}


